package robot;


import java.io.FileNotFoundException;
import java.util.List;

import javax.microedition.lcdui.Image;

import lejos.nxt.*;
import lejos.robotics.navigation.DifferentialPilot;

public class Robot{
	public boolean isForward = false; //whether or not the robot is moving forward already
	public boolean isBackward = false;
	
	private NXTRegulatedMotor left;
	private NXTRegulatedMotor right;	
	private NXTRegulatedMotor arm;
	
	/** Time/Count per degree it takes for the wheel to turn by a degree **/ 
	//private double countPerDegree = 
	//private double wheelDiameter = 5;
	private Camera cam;
	
	private RobotBehaviour strategy;
	
	private DifferentialPilot pilot;
	
	double turnRate = 100;
	int defaultAngle = 90;
	int defaultSpeed = 10; //degrees per second
	double curveTooSmall = .001;
	double curveUnit = 3.75; //cm

	/** Constructer: Initialize the Motors, and Count per Degree & sets default speed**/ 
	public Robot(){

		//Initialize Motors
		left = Motor.B;
		right = Motor.A;
		arm = Motor.C;


		//set default speed
		//left.setSpeed(defaultSpeed);
		//right.setSpeed(defaultSpeed);
		
		// DifferentialPilot is our motion controller, we will exclusively use the object to navigate the robot around
	    // the first two arguments are wheel diameters and track width (in cm) respectively
	    // last two arguments are left and right motors respectively
	    pilot = new DifferentialPilot(2.5f, 4.0f, left, right); //check wheel diameter
	    pilot.setTravelSpeed(defaultSpeed);
	    
	}

	public boolean execute(int command){
		String thecommand = Integer.toString(command);
		if (thecommand.equals("111000")) //Hold forward
		{
			//halt();
			moveForward(0); //0 means no distance specified
		}
		
		if (thecommand.startsWith("101")) //Move Forward X distance
		{
			String forwardString = thecommand.substring(3,6);
			int forwardCommand = Integer.parseInt(forwardString);
			moveForward(forwardCommand);
		}
		
		if (thecommand.equals("112000")) //Hold backward
		{
			//halt();
			moveBackward(0);
		}
		if (thecommand.startsWith("102")) //Move Backward X distance
		{
			String backwardString = thecommand.substring(3,6);
			int backwardCommand = Integer.parseInt(backwardString);
			moveBackward(backwardCommand);
		}
		if (thecommand.equals("222000")) //Turn left default degrees
		{
			turnLeft(defaultAngle);
		}
		
		if (thecommand.startsWith("202")) //turn  Left X degrees
		{
			String leftString = thecommand.substring(3, 6);	
			int leftCommand = Integer.parseInt(leftString);
			turnLeft(leftCommand);
		}
		
		if (thecommand.equals("211000"))  //Turn Right default degrees
		{
			turnRight(defaultAngle);
		}
		if (thecommand.startsWith("201")) //turn  Right X degrees
		{
			String rightString = thecommand.substring(3, 6);	
			int rightCommand = Integer.parseInt(rightString);
			turnRight(rightCommand);
		}
		if (thecommand.equals("322000")) //Hold arm Left
		{
			armLeft();
		}
		if (thecommand.equals("311000"))
		{
			armRight();
		}
		
		if (thecommand.equals("255000")) //Stop
		{
			halt();
		}
		
		if (thecommand.startsWith("701")) //Set Speed X
		{
			String speedString = thecommand.substring(3, 6);
			int speedCommand = Integer.parseInt(speedString);
			setSpeedX(speedCommand);
		}

		if (thecommand.startsWith("122")) //curve left
		{
			String rightString = thecommand.substring(3, 6);	
			int rightCommand = Integer.parseInt(rightString);
			curveLeft(rightCommand);
		}

		if (thecommand.startsWith("123")) //curve RIGHT
		{
			String rightString = thecommand.substring(3, 6);	
			int rightCommand = Integer.parseInt(rightString);
			curveRight(rightCommand);		
		}
		return true;
	}

	private void setSpeedX(int theCommand)
	{	
		pilot.setTravelSpeed(theCommand);
	}

	private void turnLeft(int i) {
		//if(isForward){
			//we should curve..
		//	curveLeft(i);
		//}else{
			//in place rotation
			pilot.rotate(i);
	
		//}		
	}
	
	private void turnRight(int i){
		//if(isForward){
			//we should curve..
		//	curveRight(i);
		//}else{
			//in place rotation
			pilot.rotate(-i);
		//}
	}

	private void armRight() 
	{
		arm.forward();
	}

	private void armLeft() {
		
		arm.backward();
	}

	public void setBehaviour(RobotBehaviour b){
		strategy=b;	
	}
	
	private List<Image> scan(){
		return null;	
	}

	private void moveForward(int distance){
		
		//we're moving forward..
		isForward = true; 

		if(distance==0){
		//left.forward();
		//right.forward();
		pilot.forward();
		//pilot.steer(distance, angle)
		}else{
			pilot.travel((double)distance);
		}
		
	}

	private void moveBackward(int distance){
		
		isBackward = true;
		if(distance==0){
			pilot.backward();
		}
		else{
		pilot.travel(-(double)distance);
		}
	} 

	/** stops the motors left and right of the NxtRovers to come to complete halt */
	public void halt(){
		//left.stop();
		//right.stop();   
		//left.stop();
		pilot.stop();
		arm.stop();
		
		//TEMPORARY
		//left.setSpeed(defaultSpeed);
		//right.setSpeed(defaultSpeed);
		isForward = false; 
		isBackward = false;

	}

	//radius is all cm
	private void curveLeft(double radius)
	{
		radius = radius/curveUnit;
		if(radius==0){
			radius=curveTooSmall;
		}
		pilot.arcForward(radius);
	}
	
	//1 unit = 3.75 cm
	//curve right if the number is positive
	//left if it's negative
	private void curveRight(double radius)
	{
		radius = radius/curveUnit;
		if(radius==0){
			radius=curveTooSmall;
		}
		pilot.arcForward(-radius);
	}
	
	//stops the the rover smoothly (not suddent break)
	private void coast(){
		left.flt();
		right.flt();   
	}


	/** Change the current speed of the NxtRover to input parameter speed, which should be a integer number
	 * between 0 and 100. * */
	private void changeRoverSpeed(int speed){
		if(speed > 0){
			left.setSpeed(speed);
			right.setSpeed(speed);
		}
		else{
			left.setSpeed(defaultSpeed);
			right.setSpeed(defaultSpeed);
		}
	}

	/** Turn the robot for the given number of milliseconds. This method is the basis of the
	 * turnLeft() and turnRight() methods */

	private void turn(int milliseconds) {

		try {
			if(milliseconds < 0) {

				left.forward();
				right.backward();
				milliseconds = -milliseconds;
			} 
			else {

				right.forward();
				left.backward();
			}
			Thread.sleep(milliseconds);
			halt();
		} 
		catch (InterruptedException e) {}
	}
//
//	/** Turn towards the left (counter-clockwise) by degrees, or clockwise if negative. */
//
//	private void turnLeft(double degrees) {
//		int count = (int)(countPerDegree * degrees);
//		turn(count);
//	}
//
//	/** Turn towards the right (clockwise) by degrees, or counter-clockwise if negative. */
//	private void turnRight(double degrees) {
//		int count = (int)(countPerDegree * degrees);
//		turn(-count);
//	}


	/** This method is basis for the methods moveForward(int milliseconds) and moveBackward(int milliseconds) * */
	private void move(int milliseconds)
			throws InterruptedException {
		if( milliseconds >= 0 ) {

			left.forward();
			right.forward();
		} else {

			left.backward();
			right.backward();
			milliseconds = -milliseconds;
		}
		Thread.sleep(milliseconds);
		halt();
	}

	/** Move forward for approximately milliseconds * */
	/*private void moveForward(int milliseconds) {
		try {
			move(milliseconds);

		} catch (InterruptedException e) {}
	}*/

	/** Move backward for approximately milliseconds*/
	private void moveBackwardTime(int milliseconds) {
		try {
			move(-milliseconds);

		} catch (InterruptedException e) {}
	}

	public void executeListofCommands(String fileName) throws FileNotFoundException
	{


	}
	
	public DifferentialPilot getPilot(){
		return this.pilot;
	}
}